#include"ros/ros.h"
#include"turtlesim/Pose.h"
#include"tf2_ros/transform_broadcaster.h"
#include"geometry_msgs/TransformStamped.h"
#include"tf2/LinearMath/Quaternion.h"
void domsg(const turtlesim::Pose::ConstPtr &p)
{ 
   static tf2_ros::TransformBroadcaster ruler;
   geometry_msgs::TransformStamped h;
   h.child_frame_id="turtle1";
   h.header.frame_id="world";
   h.header.stamp=ros::Time::now();
   h.transform.translation.x=p->x;
   h.transform.translation.y=p->y;
   h.transform.translation.z=0;
   
   tf2::Quaternion q;
   q.setRPY(0,0,p->theta);
   h.transform.rotation.x=q.getX();
   h.transform.rotation.y=q.getY();
   h.transform.rotation.z=q.getZ();
   h.transform.rotation.w=q.getW();
   
   ruler.sendTransform(h);

}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"jdg");
    ros::NodeHandle nh;
    ros::Subscriber sub=nh.subscribe("/turtle1/pose",10,domsg);
    ros::spin();
    return 0;
}
